/*
MultiWiiCopter by Alexandre Dubus
www.multiwii.com
April  2011     V1.7
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 any later version. see <http://www.gnu.org/licenses/>
*/

#include "defines.h"
#include "AP_Config.h"
#include "AP_PID_settings.h"

/*******************************/
/****CONFIGURABLE PARAMETERS****/
/*******************************/

/* Set the minimum throttle command sent to the ESC (Electronic Speed Controller)
   This is the minimum value that allow motors to run at a idle speed  */
//#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A
#define MINTHROTTLE 1120 // for Super Simple ESCs 10A
//#define MINTHROTTLE 1190


#define FLYING_WING //experimental

#define YAW_DIRECTION 1 // if you want to reverse the yaw correction direction
//#define YAW_DIRECTION -1

#define I2C_SPEED 100000L     //100kHz normal mode, this value must be used for a genuine WMP
//#define I2C_SPEED 400000L   //400kHz fast mode, it works only with some WMP clones

#define PROMINI  //Arduino type

//enable internal I2C pull ups
#define INTERNAL_I2C_PULLUPS

//****** advanced users settings   *************

/* Failsave settings - added by MIS
   Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated.
   After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered
   and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results. 
   This value is depended from your configuration, AUW and some other params. 
   Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped.
   If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal.
   If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/
#define FAILSAFE                                  // Alex: comment this line if you want to deactivate the failsafe function
#define FAILSAVE_DELAY     10                     // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example
#define FAILSAVE_OFF_DELAY 200                    // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example
#define FAILSAVE_THR0TTLE  (MINTHROTTLE + 200)    // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case


/* The following lines apply only for a pitch/roll tilt stabilization system
   It is not compatible with Y6 or HEX6 or HEX6X
   Uncomment the first line to activate it */
//#define SERVO_TILT
#define TILT_PITCH_MIN    1020    //servo travel min, don't set it below 1020
#define TILT_PITCH_MAX    2000    //servo travel max, max value=2000
#define TILT_PITCH_MIDDLE 1500    //servo neutral value
#define TILT_PITCH_PROP   10      //servo proportional (tied to angle) ; can be negative to invert movement
#define TILT_ROLL_MIN     1020
#define TILT_ROLL_MAX     2000
#define TILT_ROLL_MIDDLE  1500
#define TILT_ROLL_PROP    10

/* The following lines apply only for specific receiver with only one PPM sum signal, on digital PIN 2
   IF YOUR RECEIVER IS NOT CONCERNED, DON'T UNCOMMENT ANYTHING. Note this is mandatory for a Y6 setup
   Select the right line depending on your radio brand. Feel free to modify the order in your PPM order is different */
//#define SERIAL_SUM_PPM         PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,CAMPITCH,CAMROLL //For Graupner/Spektrum
//#define SERIAL_SUM_PPM         ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL //For Robe/Hitec/Futaba
//#define SERIAL_SUM_PPM         PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL //For some Hitec/Sanwa/Others

/* interleaving delay in micro seconds between 2 readings WMP/NK in a WMP+NK config
   if the ACC calibration time is very long (20 or 30s), try to increase this delay up to 4000
   it is relevent only for a conf with NK */
#define INTERLEAVING_DELAY 3000

/* for V BAT monitoring
   after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN
   with R1=33k and R2=51k
   vbat = [0;1023]*16/VBATSCALE */
#define VBAT              // comment this line to suppress the vbat code
#define VBATSCALE     131 // change this value if readed Battery voltage is different than real voltage
#define VBATLEVEL1_3S 107 // 10,7V
#define VBATLEVEL2_3S 103 // 10,3V
#define VBATLEVEL3_3S 99  // 9.9V

/* when there is an error on I2C bus, we neutralize the values during a short time. expressed in microseconds
   it is relevent only for a conf with at least a WMP */
#define NEUTRALIZE_DELAY 100000

/* this is the value for the ESCs when thay are not armed
   in some cases, this value must be lowered down to 900 for some specific ESCs */
#define MINCOMMAND 1000

/* this is the maximum value for the ESCs at full power
   this value can be increased up to 2000 */
#define MAXTHROTTLE 1850

/* This is the speed of the serial interface. 115200 kbit/s is the best option for a USB connection.*/
#define SERIAL_COM_SPEED 115200

/* In order to save space, it's possibile to desactivate the LCD configuration functions
   comment this line only if you don't plan to used a LCD */
#define LCD_CONF

/* to use Cat's whisker TEXTSTAR LCD, uncomment following line.
   Pleae note this display needs a full 4 wire connection to (+5V, Gnd, RXD, TXD )
   Configure display as follows: 115K baud, and TTL levels for RXD and TXD, terminal mode
   NO rx / tx line reconfiguration, use natural pins */
//#define LCD_TEXTSTAR

/* motors will not spin when the throttle command is in low position
   this is an alternative method to stop immediately the motors */
//#define MOTOR_STOP

/* some radios have not a neutral point centered on 1500. can be changed here */
#define MIDRC 1500

/* experimental
   camera trigger function : activated via AUX1 UP, servo output=A2 */
//#define CAMTRIG
#define CAM_SERVO_HIGH 2000  // the position of HIGH state servo
#define CAM_SERVO_LOW 1020   // the position of LOW state servo
#define CAM_TIME_HIGH 1000   // the duration of HIGH state servo expressed in ms
#define CAM_TIME_LOW 1000    // the duration of LOW state servo expressed in ms

/* you can change the tricopter servo travel here */
#define TRI_YAW_CONSTRAINT_MIN 1020
#define TRI_YAW_CONSTRAINT_MAX 2000
#define TRI_YAW_MIDDLE 1500

//****** end of advanced users settings *************

/**************************************/
/****END OF CONFIGURABLE PARAMETERS****/
/**************************************/

#include <EEPROM.h>

#define LEDPIN 13     // will be changed for MEGA in a future version
#define POWERPIN 12   // will be changed for MEGA in a future version
#define V_BATPIN 3    // Analog PIN 3
#define STABLEPIN     // will be defined for MEGA in a future version

#if defined(PROMINI)
  #define LEDPIN_PINMODE             pinMode (13, OUTPUT);
  #define LEDPIN_SWITCH              PINB |= 1<<5;     //switch LEDPIN state (digital PIN 13)
  #define LEDPIN_OFF                 PORTB &= ~(1<<5);
  #define LEDPIN_ON                  PORTB |= (1<<5);
  #define BUZZERPIN_PINMODE          pinMode (8, OUTPUT);
  #define BUZZERPIN_ON               PORTB |= 1;
  #define BUZZERPIN_OFF              PORTB &= ~1;
  #define POWERPIN_PINMODE           pinMode (15, OUTPUT);
  #define GROUNDPIN_PINMODE          pinMode (14, OUTPUT);
  #define GROUNDPIN_ON               PORTC &= ~(1<<0);
  #define POWERPIN_ON                PORTC |= 1<<1;
  #define POWERPIN_OFF               PORTC &= ~(1<<1); //switch OFF WMP, digital PIN 12
  #define I2C_PULLUPS_ENABLE         PORTC |= 1<<4; PORTC |= 1<<5;   // PIN A4&A5 (SDA&SCL)
  #define I2C_PULLUPS_DISABLE        PORTC &= ~(1<<4); PORTC &= ~(1<<5);
  #define PINMODE_LCD                pinMode(0, OUTPUT);
  #define LCDPIN_OFF                 PORTD &= ~1;
  #define LCDPIN_ON                  PORTD |= 1;
  #define DIGITAL_SERVO_TRI_PINMODE  pinMode(3,OUTPUT); //also right servo for BI COPTER
  #define DIGITAL_SERVO_TRI_HIGH     PORTD |= 1<<3;
  #define DIGITAL_SERVO_TRI_LOW      PORTD &= ~(1<<3);
  #define DIGITAL_TILT_PITCH_PINMODE pinMode(A0,OUTPUT);
  #define DIGITAL_TILT_PITCH_HIGH    PORTC |= 1<<0;
  #define DIGITAL_TILT_PITCH_LOW     PORTC &= ~(1<<0);
  #define DIGITAL_TILT_ROLL_PINMODE  pinMode(A1,OUTPUT);
  #define DIGITAL_TILT_ROLL_HIGH     PORTC |= 1<<1;
  #define DIGITAL_TILT_ROLL_LOW      PORTC &= ~(1<<1);
  #define DIGITAL_BI_LEFT_PINMODE    pinMode(11,OUTPUT); 
  #define DIGITAL_BI_LEFT_HIGH       PORTB |= 1<<3;
  #define DIGITAL_BI_LEFT_LOW        PORTB &= ~(1<<3);
  #define PPM_PIN_INTERRUPT          attachInterrupt(0, rxInt, RISING); //PIN 0
  #define MOTOR_ORDER                9,10,11,3,6,5  //for a quad+: rear,right,left,front
  #define DIGITAL_CAM_PINMODE        pinMode(A2,OUTPUT);
  #define DIGITAL_CAM_HIGH           PORTC |= 1<<2;
  #define DIGITAL_CAM_LOW            PORTC &= ~(1<<2);
  //RX PIN assignment inside the port //for PORTD
  #define THROTTLEPIN                2
  #define ROLLPIN                    4
  #define PITCHPIN                   5
  #define YAWPIN                     6
  #define AUX1PIN                    7
  #define AUX2PIN                    7   //unused just for compatibility with MEGA
  #define CAM1PIN                    7   //unused just for compatibility with MEGA
  #define CAM2PIN                    7   //unused just for compatibility with MEGA
  #define ISR_UART                   ISR(USART_UDRE_vect)
#endif

/*********** RC alias *****************/
#define ROLL       0
#define PITCH      1
#define YAW        2
#ifndef TRUE
#define TRUE          (0x01)
#endif
#ifndef FALSE
#define FALSE         (0x00)
#endif
#define ToRad(x) (x*0.01745329252)  // *pi/180
#define ToDeg(x) (x*57.2957795131)  // *180/pi
#define Gyro_Gain_X 0.05 //X axis Gyro gain
#define Gyro_Gain_Y 0.05 //Y axis Gyro gain
#define Gyro_Gain_Z 0.05 //Z axis Gyro gain
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second


static uint32_t previousTime;
static uint32_t neutralizeTime;
static uint32_t currentTime;
static uint16_t calibratingA;       // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static uint16_t calibratingG;
static uint16_t cycleTime;          // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
static int16_t acc_1G = 200;       //this is the 1G measured acceleration (nunchuk)
static int16_t acc_25deg = 85;     //this is the the ACC value measured on x or y axis for a 25deg inclination (nunchuk) = acc_1G * sin(25)
static uint8_t nunchukPresent = 0;
static uint8_t accPresent = 0;     //I2C or ADC acc present
static int16_t accADC[3];
static int16_t gyroADC[3];

float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0,0,0};//Omega Proportional correction
float Omega_I[3]= {0,0,0};//Omega Integrator
float Omega[3]= {0,0,0};
float G_Dt=0.02;    // Integration time (DCM algorithm)

float DCM_Matrix[3][3]= {
  {
    1,0,0  }
  ,{
    0,1,0  }
  ,{
    0,0,1  }
}; 
float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here

float Temporary_Matrix[3][3]={
  {
    0,0,0  }
  ,{
    0,0,0  }
  ,{
    0,0,0  }
};

// Euler angles
float roll;
float pitch;
float yaw;

int toggleMode=0;

float errorRollPitch[3]= {0,0,0}; 
float errorYaw[3]= {0,0,0};
float errorCourse=180; 
float COGX=0; //Course overground X axis
float COGY=1; //Course overground Y axis

// #define Kp_ROLLPITCH 0.015
// #define Ki_ROLLPITCH 0.000010
// #define Kp_YAW 1.2
// #define Ki_YAW 0.00005
#define Kp_ROLLPITCH 0.05967 		// .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain
#define Ki_ROLLPITCH 0.00001278		// 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
#define Kp_YAW 0.8		 			// Yaw Drift Correction Porportional Gain	
#define Ki_YAW 0.00004 				// Yaw Drift CorrectionIntegrator Gain


#define GRAVITY 200 //this equivalent to 1G in the raw data coming from the accelerometer 
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square

/* Radio values
		Channel assignments	
			1	Ailerons (rudder if no ailerons)
			2	Elevator
			3	Throttle
			4	Rudder (if we have ailerons)
*/
int16_t radio_min[] 	= {CH1_MIN, CH2_MIN, CH3_MIN, CH4_MIN};	// may be reset by init sequence
int16_t radio_trim[] 	= {0,0,0,0};							// may be reset by init sequence
int16_t radio_max[] 	= {CH1_MAX, CH2_MAX, CH3_MAX, CH4_MAX};	// may be reset by init sequence

int16_t radio_in[]		= {1500,1500,1100,1500};			// current values from the transmitter - microseconds
int16_t radio_out[]		= {1500,1500,1100,1500};			// PWM to ROLL PITCH Servos
float servo_out[] 	= {0,0,0,0};						// current values to the servos - -45 to 45 degrees, except [3] is 0 to 100

int16_t elevon1_trim 	= 1500;
int16_t elevon2_trim 	= 1500;
int16_t ch1_temp 		= 1500;			// Used for elevon mixing
int16_t ch2_temp 		= 1500;
int16_t ch3_raw;

uint16_t timer_ovf_a	= 0;
uint16_t timer_ovf_b	= 0;
uint16_t timer_ovf		= 0;

float kp[]={SERVO_ROLL_P, SERVO_PITCH_P, SERVO_RUDDER_P, NAV_ROLL_P, NAV_PITCH_ASP_P, NAV_PITCH_ALT_P, THROTTLE_TE_P, THROTTLE_ALT_P};
float ki[]={SERVO_ROLL_I, SERVO_PITCH_I, SERVO_RUDDER_I, NAV_ROLL_I, NAV_PITCH_ASP_I, NAV_PITCH_ALT_I, THROTTLE_TE_I, THROTTLE_ALT_I};
float kd[]={SERVO_ROLL_D, SERVO_PITCH_D, SERVO_RUDDER_D, NAV_ROLL_D, NAV_PITCH_ASP_D, NAV_PITCH_ALT_D, THROTTLE_TE_D, THROTTLE_ALT_D};

const float integrator_max[] = {SERVO_ROLL_INT_MAX, SERVO_PITCH_INT_MAX, SERVO_RUDDER_INT_MAX, NAV_ROLL_INT_MAX, NAV_PITCH_ASP_INT_MAX, NAV_PITCH_ALT_INT_MAX, THROTTLE_TE_INT_MAX, THROTTLE_ALT_INT_MAX};

byte 	control_mode		= MANUAL;


float	integrator[] = {0,0,0,0,0,0,0,0};	// PID Integrators
float	last_error[] = {0,0,0,0,0,0,0,0};	// PID last error for derivative
float 	nav_gain_scaler	= 1;			// Gain scaling for headwind/tailwind

// these are the values for navigation control functions
// ----------------------------------------------------
long 	nav_roll			= 0;			// deg * 100 : target roll angle
long 	nav_pitch			= 0;			// deg * 100 : target pitch angle
int 	throttle_cruise		= THROTTLE_CRUISE;	// 0-100 : target throttle output for average speed
long	altitude_estimate	= 0;			// for smoothing GPS output

long roll_sensor			= 0;		// how much we're turning in deg * 100
long pitch_sensor			= 0;		// our angle of attack in deg * 100

// Location & Navigation 
// ---------------------
byte 	wp_radius			= 20;			// meters
int 	ground_speed 		= 0;			// m/s * 100
long 	ground_course 		= 0;			// deg * 100 dir of plane
long 	hold_course 		= -1;			// deg * 100 dir of plane
long	nav_bearing			= 0;			// deg * 100 : 0 to 360 current desired bearing to navigate
long 	target_bearing		= 0;			// deg * 100 : 0 to 360 location of the plane to the target
long 	crosstrack_bearing	= 0;			// deg * 100 : 0 to 360 desired angle of plane to target
int 	climb_rate 			= 0;			// m/s * 100
byte	loiter_radius 		= LOITER_RADIUS; // meters


// Airspeed
// --------
float 	airspeed 			= 0; 							// m/s * 100
int		airspeed_cruise		= AIRSPEED_CRUISE * 100;		// m/s * 100 : target airspeed sensor value

// Location Errors
// ---------------
long 	bearing_error		= 0; 			// deg * 100 : 18000 to -18000 
long 	altitude_error		= 0;			// meters * 100 we are off in altitude
float 	airspeed_error		= 0;			// m/s * 100: 
float	crosstrack_error	= 0;			// deg * 100 : 18000 to -18000  meters we are off trackline
long 	energy_error		= 0;


// System Timers
// --------------
unsigned long fast_loopTimer		= 0;		// Time in miliseconds of main control loop
unsigned long medium_loopTimer		= 0;		// Time in miliseconds of navigation control loop
byte medium_loopCounter				= 0;		// Counters for branching from main control loop to slower loops
byte slow_loopCounter				= 0;		// 
unsigned long deltaMiliSeconds 		= 0;		// Delta Time in miliseconds
unsigned long dTnav					= 0;		// Delta Time in milliseconds for navigation computations
int mainLoop_count 					= 0;
unsigned long elapsedTime			= 0;		// for doing custom events

// *********************
// I2C general functions
// *********************

// Mask prescaler bits : only 5 bits of TWSR defines the status of each I2C request
#define TW_STATUS_MASK	(1<<TWS7) | (1<<TWS6) | (1<<TWS5) | (1<<TWS4) | (1<<TWS3)
#define TW_STATUS       (TWSR & TW_STATUS_MASK)

void i2c_init(void) {
  #if defined(INTERNAL_I2C_PULLUPS)
    I2C_PULLUPS_ENABLE
  #else
    I2C_PULLUPS_DISABLE
  #endif
  TWSR = 0;        // no prescaler => prescaler = 1
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
  TWCR = 1<<TWEN;  // enable twi module, no interrupt
}

void i2c_rep_start(uint8_t address) {
  TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWSTO); // send REAPEAT START condition
  waitTransmissionI2C(); // wait until transmission completed
  checkStatusI2C(); // check value of TWI Status Register
  TWDR = address; // send device address
  TWCR = (1<<TWINT) | (1<<TWEN);
  waitTransmissionI2C(); // wail until transmission completed
  checkStatusI2C(); // check value of TWI Status Register
}

void i2c_write(uint8_t data ) {	
  TWDR = data; // send data to the previously addressed device
  TWCR = (1<<TWINT) | (1<<TWEN);
  waitTransmissionI2C(); // wait until transmission completed
  checkStatusI2C(); // check value of TWI Status Register
}

uint8_t i2c_readAck() {
  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
  waitTransmissionI2C();
  return TWDR;
}

uint8_t i2c_readNak(void) {
  TWCR = (1<<TWINT) | (1<<TWEN);
  waitTransmissionI2C();
  return TWDR;
}

void waitTransmissionI2C() {
  uint8_t count = 255;
  while (count-->0 && !(TWCR & (1<<TWINT)) );
  if (count<2) { //we are in a blocking state => we don't insist
    TWCR = 0;  //and we force a reset on TWINT register
    neutralizeTime = micros(); //we take a timestamp here to neutralize the value during a short delay after the hard reset
  }
}

void checkStatusI2C() {
  if ( TW_STATUS  == 0xF8) { //TW_NO_INFO : this I2C error status indicates a wrong I2C communication.
    // WMP does not respond anymore => we do a hard reset. I did not find another way to solve it. It takes only 13ms to reset and init to WMP or WMP+NK
    TWCR = 0;
    POWERPIN_OFF //switch OFF WMP
    delay(1);  
    POWERPIN_ON  //switch ON WMP
    delay(10);
    #if defined(ITG3200) || defined(L3G4200D)
    #else
      i2c_WMP_init(0);
    #endif
    neutralizeTime = micros(); //we take a timestamp here to neutralize the WMP or WMP+NK values during a short delay after the hard reset
  }
}

// **************************
// I2C Wii Motion Plus 
// **************************
// I2C adress 1: 0xA6 (8bit)    0x53 (7bit)
// I2C adress 2: 0xA4 (8bit)    0x52 (7bit)

static uint8_t rawADC_WMP[6];

void i2c_WMP_init(uint8_t d) {
  delay(d);
  i2c_rep_start(0xA6 + 0);//I2C write direction => 0
  i2c_write(0xF0); 
  i2c_write(0x55); 
  delay(d);
  i2c_rep_start(0xA6 + 0);//I2C write direction => 0
  i2c_write(0xFE); 
  i2c_write(0x05); 
  delay(d);
  if (d>0) {
    uint8_t numberAccRead = 0;
    for(uint8_t i=0;i<100;i++) {
      delay(3);
      if (rawIMU(0) == 0) numberAccRead++; // we detect here is nunchuk extension is available
    }
    if (numberAccRead>25)
      nunchukPresent = 1;
    delay(10);
  }
}

void i2c_WMP_getRawADC() {
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
  i2c_rep_start(0xA4 + 0);//I2C write direction => 0
  i2c_write(0x00);
  i2c_rep_start(0xA4 + 1);//I2C read direction => 1
  for(uint8_t i = 0; i < 5; i++)
    rawADC_WMP[i]=i2c_readAck();
  rawADC_WMP[5]= i2c_readNak();
}

// **************
// gyro+acc IMU
// **************
static int16_t gyroData[3] = {0,0,0};
static int16_t gyroZero[3] = {0,0,0};
static int16_t accZero[3]  = {0,0,0};
static int16_t angle[2];      //absolute angle inclination in multiple of 0.1 degree
static int16_t accSmooth[3];  //projection of smoothed and normalized gravitation force vector on x/y/z axis, as measured by accelerometer


uint8_t rawIMU(uint8_t withACC) { //if the WMP or NK are oriented differently, it can be changed here
    i2c_WMP_getRawADC();
    if ( (rawADC_WMP[5]&0x02) == 0x02 && (rawADC_WMP[5]&0x01) == 0 ) {// motion plus data
      gyroADC[PITCH] =  ( ((rawADC_WMP[5]>>2)<<8) + rawADC_WMP[2] );
      gyroADC[ROLL]  = ( ((rawADC_WMP[4]>>2)<<8) + rawADC_WMP[1] );
      gyroADC[YAW]   = -( ((rawADC_WMP[3]>>2)<<8) + rawADC_WMP[0] );
      return 1;
    } else if ( (rawADC_WMP[5]&0x02) == 0 && (rawADC_WMP[5]&0x01) == 0) { //nunchuk data
        accADC[PITCH] =  ( (rawADC_WMP[3]<<2)        + ((rawADC_WMP[5]>>4)&0x2) );
        accADC[ROLL]  =  ( (rawADC_WMP[2]<<2)        + ((rawADC_WMP[5]>>3)&0x2) );
        accADC[YAW]   = -( ((rawADC_WMP[4]&0xFE)<<2) + ((rawADC_WMP[5]>>5)&0x6) );
        return 0;
    } else
      return 2;
}

uint8_t updateIMU(uint8_t withACC) {
  static int32_t g[3];
  static int32_t a[3];
  uint8_t axis;
  static int16_t previousGyroADC[3] = {0,0,0};
  uint8_t r;
  r=rawIMU(withACC);
  
  if (currentTime < (neutralizeTime + NEUTRALIZE_DELAY)) {//we neutralize data in case of blocking+hard reset state
    for (axis = 0; axis < 3; axis++) {gyroADC[axis]=0;accADC[axis]=0;}
    accADC[YAW] = acc_1G;
  } else {
    if (r == 1) { //gyro
      if (calibratingG>0) {
        for (axis = 0; axis < 3; axis++) {
          if (calibratingG>1) {
            if (calibratingG == 400) g[axis]=0;
            g[axis] +=gyroADC[axis];
            gyroADC[axis]=0;
          } else {
            gyroZero[axis]=(g[axis]+200)/399;
            blinkLED(10,15,1+3*nunchukPresent);
          }
        }
        calibratingG--;
      }
    gyroADC[ROLL]  = gyroADC[ROLL]  - gyroZero[ROLL];
    gyroADC[PITCH] = gyroADC[PITCH] - gyroZero[PITCH];
    gyroADC[YAW]   = gyroADC[YAW]   - gyroZero[YAW];
    gyroADC[ROLL]  = (rawADC_WMP[3]&0x01)     ? gyroADC[ROLL]/5  : gyroADC[ROLL] ;   //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification 
    gyroADC[PITCH] = (rawADC_WMP[4]&0x02)>>1  ? gyroADC[PITCH]/5 : gyroADC[PITCH] ;  //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details)
    gyroADC[YAW]   = (rawADC_WMP[3]&0x02)>>1  ? gyroADC[YAW]/5   : gyroADC[YAW] ;

      //anti gyro glitch, limit the variation between two consecutive readings
      for (axis = 0; axis < 3; axis++) {
        gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-100,previousGyroADC[axis]+100);
        previousGyroADC[axis] = gyroADC[axis];
      }
    }
    if (r == 0 || ( (accPresent == 1) && (withACC == 1) ) ) { //nunchuk or i2c ACC
      if (calibratingA>0) {
        if (calibratingA>1) {
          for (uint8_t axis = 0; axis < 3; axis++) {
            if (calibratingA == 400) a[axis]=0;
            a[axis] +=accADC[axis];
            accADC[axis]=0;
          }
        } else {
          accZero[ROLL]  = (a[ROLL]+200)/399;
          accZero[PITCH] = (a[PITCH]+200)/399;
          accZero[YAW]   = (a[YAW]+200)/399+acc_1G; // for nunchuk 200=1G
        }
        calibratingA--;
      } else {
        accADC[ROLL]  = accADC[ROLL]  - accZero[ROLL] ;
        accADC[PITCH] = accADC[PITCH] - accZero[PITCH];
        accADC[YAW]   = - (accADC[YAW]   - accZero[YAW]) ;
      }
    }
  }  
  return r;
}


void blinkLED(uint8_t num, uint8_t wait,uint8_t repeat) {
  uint8_t i,r;
  for (r=0;r<repeat;r++) {
    for(i=0;i<num;i++) {
      LEDPIN_SWITCH //switch LEDPIN state
      BUZZERPIN_ON
      delay(wait);
      BUZZERPIN_OFF
    }
    delay(60);
  }
}


void setup() {
  Serial.begin(SERIAL_COM_SPEED);
  
	pinMode(2,INPUT);	// PD2 - INT0 		- Rudder in							- INPUT Rudder/Aileron
	pinMode(3,INPUT);	// PD3 - INT1 		- Elevator in 						- INPUT Elevator
	pinMode(4,INPUT);	// PD4 - XCK/T0 	- MUX pin							- Connected to Pin 2 on ATtiny
	pinMode(5,INPUT);	// PD5 - T0			- Mode pin							- Connected to Pin 6 on ATtiny   - Select on MUX
	pinMode(6,OUTPUT);	// PD6 - T1			- Ground start signaling Pin	
	pinMode(7,OUTPUT);	// PD7 - AIN0		- GPS Mux pin 
	pinMode(8, OUTPUT); // PB0 - AIN1		- Servo throttle					- OUTPUT THROTTLE
	pinMode(9, OUTPUT);	// PB1 - OC1A		- Elevator PWM out					- Elevator PWM out
	pinMode(10,OUTPUT);	// PB2 - OC1B		- Rudder PWM out					- Aileron PWM out
	pinMode(11,INPUT); 	// PB3 - MOSI/OC2	-  
	pinMode(12,OUTPUT); // PB4 - MISO		- Blue LED pin  - GPS Lock			- GPS Lock
	pinMode(13,INPUT); 	// PB5 - SCK		- Yellow LED pin   					- INPUT Throttle


  
  LEDPIN_PINMODE
  POWERPIN_PINMODE
  POWERPIN_OFF
  GROUNDPIN_PINMODE
  GROUNDPIN_ON
  BUZZERPIN_PINMODE
  POWERPIN_OFF
  delay(200);
  POWERPIN_ON
  delay(100);
  i2c_init();
  delay(100);
  
  i2c_WMP_init(250);
  
  // connect to radio 
  // ----------------
  init_radio();

  // setup PWM timers
  // ----------------
  init_PWM();

  
  previousTime = micros();
  calibratingA = 400;
  calibratingG = 400;
  
  control_mode = STABILIZE;
}

void Filter_Sensors(float gyroFactor, float accelFactor)
{
	uint8_t axis;  
	for (axis=0;axis<3;axis++) Gyro_Vector[axis] =(Gyro_Vector[axis]*(gyroFactor-1)+gyroADC[axis])/gyroFactor;
	for (axis=0;axis<3;axis++) Accel_Vector[axis] =(Accel_Vector[axis]*(accelFactor-1)+accADC[axis])/accelFactor;
}

// ******** Main Loop *********
void loop () { 
	
  static uint16_t tPrevious;
  static uint16_t tServoUpdatePrevious;
  uint16_t tCurrent,G_Dt;
  updateIMU(0);
  tCurrent = millis();
  G_Dt = (tCurrent-tPrevious)*0.001;
  deltaMiliSeconds = tCurrent-tPrevious;
  tPrevious = tCurrent;
  
  
  Filter_Sensors(2,2); //Tuned values for WM+
  Matrix_update(); 
  Normalize();
  Drift_correction();
  Euler_angles();
  
  roll_sensor = int(roll * 100);
  pitch_sensor = int(pitch * 100);
  
//   Serial.print(gyroADC[ROLL]);Serial.print(',');
//   Serial.print(gyroADC[PITCH]);Serial.print(',');
//   Serial.print(gyroADC[YAW]);Serial.print(',');
//   Serial.print(accADC[ROLL]);Serial.print(',');
//   Serial.print(accADC[PITCH]);Serial.print(',');
//   Serial.print(accADC[YAW]);Serial.print(',');
//   Serial.print((float)ToDeg(roll));Serial.print(',');
//   Serial.print((float)ToDeg(pitch));Serial.print(',');
//   Serial.print((float)ToDeg(yaw));Serial.println();
  

  //if((tCurrent-tServoUpdatePrevious) > 19) {
	tServoUpdatePrevious = tCurrent;
    read_radio();
    stabilize();
    set_servos_4();
//     Serial.print(servo_out[CH_ROLL]);Serial.print('\t');
// 	Serial.print(servo_out[CH_PITCH]);Serial.print('\t');
//	Serial.println();
	
  //}


    // Measure loop rate just afer reading the sensors
  currentTime = micros();
  cycleTime = currentTime - previousTime;
  previousTime = currentTime;
  delay(12);
}

static uint8_t point;
static uint8_t s[128];
void serialize16(int16_t a) {s[point++]  = a; s[point++]  = a>>8&0xff;}
void serialize8(uint8_t a)  {s[point++]  = a;}

// ***********************************
// Interrupt driven UART transmitter for MIS_OSD
// ***********************************
static uint8_t tx_ptr;

ISR_UART {
  UDR0 = s[tx_ptr++];         /* Transmit next byte */
  if ( tx_ptr == point )        /* Check if all data is transmitted */
    UCSR0B &= ~(1<<UDRIE0);     /* Disable transmitter UDRE interrupt */
}

void UartSendData() {      // start of the data block transmission
  tx_ptr = 0;
  UCSR0A |= (1<<UDRE0);        /* Clear UDRE interrupt flag */
  UCSR0B |= (1<<UDRIE0);       /* Enable transmitter UDRE interrupt */
  UDR0 = s[tx_ptr++];          /* Start transmission */
}
